/*
 * Copyright 2011 Marcin Blazejewski
 *
 * This file is part of Ground0.
 *
 * Ground0 is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * Ground0 is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with Ground0. If not, see <http://www.gnu.org/licenses/>.
 *
 */

#include <config.h>
#include "geometry.hpp"
#include "quat.hpp"

#include <string.h>
#include <math.h>
#include <iostream>

using namespace std;

Vector Vector::x_axis(1, 0, 0);
Vector Vector::y_axis(0, 1, 0);
Vector Vector::z_axis(0, 0, 1);

Vector face_normal(Vector &v0, Vector &v1, Vector &v2) {
    Vector n;
    face_normal(v0, v1, v2, n);
    return n;
}

Vector& face_normal(Vector &v0, Vector &v1, Vector &v2, Vector &n) {
    Vector a, b;

    // get 2 vetors a, b defining the face
    a.x = v2.x - v0.x;
    a.y = v2.y - v0.y;
    a.z = v2.z - v0.z;

    b.x = v1.x - v0.x;
    b.y = v1.y - v0.y;
    b.z = v1.z - v0.z;
 
    // normal vector = cross product of a and b
    // n = a x b
    n.x = (a.y * b.z) - (a.z * b.y);
    n.y = (a.z * b.x) - (a.x * b.z);
    n.z = (a.x * b.y) - (a.y * b.x);

    // normalize
    n.normalize();
    n *= -1;

    return n;
}

Vector Vector::from_polar(float r, float heading, 
                          float elevation) {
    Vector v;
    v.set_polar(r, heading, elevation);
    return v;
}

Vector& Vector::set_polar(float r, float heading, float elevation) {
    float cast = r * cos(elevation);
    x = cast * cos(heading);
    y = r * sin(elevation);
    z = cast * sin(heading);
    return *this;
}


Vector::Vector(float _x, float _y, float _z)
    : x(_x), y(_y), z(_z) {
}

Vector& Vector::operator +=(const Vector &v) {
    x += v.x;
    y += v.y;
    z += v.z;

    return *this;
}

Vector& Vector::operator -=(const Vector &v) {
    x -= v.x;
    y -= v.y;
    z -= v.z;

    return *this;
}

Vector& Vector::operator *=(const float f) {
    x *= f;
    y *= f;
    z *= f;

    return *this;
}

Vector& Vector::operator /=(const float f) {
    x /= f;
    y /= f;
    z /= f;

    return *this;
}

Vector Vector::operator +(const Vector &v) {
    Vector x(*this);
    x += v;
    return x;
}

Vector Vector::operator -(const Vector &v) {
    Vector x(*this);
    x -= v;
    return x;
}

Vector Vector::operator *(const float a) {
    Vector x(*this);
    x *= a;
    return x;
}

Vector Vector::operator /(const float a) {
    Vector x(*this);
    x /= a;
    return x;
}


bool Vector::operator ==(const Vector &v) {
    return (x == v.x) && (y == v.y) && (z == v.z);
}

float Vector::dot(const Vector &r) {
    return x*r.x + y*r.y + z*r.z;
}


Vector Vector::cross(const Vector &r) {
    return Vector( y * r.z - z * r.y, z * r.x - x * r.z, x * r.y - y * r.x );
}

float Vector::length() {
    return (float)sqrt( (x*x) + (y*y) + (z*z) );
}

void Vector::normalize() {
    float len = (float)sqrt( (x*x) + (y*y) + (z*z) );
 
    if (len != 0.0f) {
        x /= len;
        y /= len;
        z /= len;
    }
}

Vector Vector::get_normal() const {
    Vector v(*this);
    v.normalize();
    return v;
}

float Vector::distance2_to(const Vector &v) const {
    return ( (x - v.x) * (x - v.x) + (y - v.y) * (y - v.y) + (z - v.z) * (z - v.z) );
}

Vector Vector::left_multiply(const float A[][3]) const {
    float B[] = { x, y, z };   // matrix to represent this vector
    float r[] = { 0, 0, 0 };  // return vector matrix
    int i, j;
    for (i = 0; i < 3; i++) {
        for (j = 0; j < 3; j++) {
            r[i] += A[i][j] * B[j];
        }
    }
    
    return Vector(r[0], r[1], r[2]);
}

ostream& operator<<(ostream &o, const Vector &v) {
    o << "vector(" << v.x << ", " << v.y << ", " << v.z << ")";
    return o;
}


Vector& Vector::operator*=(const Quaternion &q) {
    Quaternion tmp(*this);
    tmp *= q.get_conjugate();
    Quaternion result = q * tmp;

    x = result.x;
    y = result.y;
    z = result.z;

    return *this;
}

Vector Vector::operator* (const Quaternion &q) const {
    Quaternion result = q * Quaternion(*this) * q.get_conjugate();

    return Vector(result.x, result.y, result.z);
}



/*******************************************************************************/
/* Matrix */
/*******************************************************************************/

float mat_det_3x3(const float A[][3]) {
    return A[0][0] * A[1][1] * A[2][2]
        + A[1][0] * A[2][1] * A[0][2]
        + A[2][0] * A[0][1] * A[1][2]
        + A[0][0] * A[2][1] * A[1][2]
        + A[2][0] * A[1][1] * A[0][2]
        + A[1][0] * A[0][1] * A[2][2];
}

void mat_inverse_3x3(const float A[][3], float ret[][3]) {
    float invdet = 1.0f / mat_det_3x3(A);
    ret[0][0] =  (A[1][1]*A[2][2]-A[1][2]*A[2][1]) * invdet;
    ret[0][1] = -(A[1][0]*A[2][2]-A[2][0]*A[1][2]) * invdet;
    ret[0][3] =  (A[1][0]*A[2][1]-A[2][0]*A[1][1]) * invdet;
    ret[1][0] = -(A[0][1]*A[2][2]-A[2][1]*A[0][2]) * invdet;
    ret[1][1] =  (A[0][0]*A[2][2]-A[2][0]*A[0][2]) * invdet;
    ret[1][2] = -(A[0][0]*A[2][1]-A[0][1]*A[2][0]) * invdet;
    ret[2][0] =  (A[0][1]*A[1][2]-A[0][2]*A[1][1]) * invdet;
    ret[2][1] = -(A[0][0]*A[1][2]-A[0][2]*A[1][0]) * invdet;
    ret[2][2] =  (A[0][0]*A[1][1]-A[0][1]*A[1][0]) * invdet;
}

ostream& operator <<(ostream &o, const float A[][3]) {
    int i, j;
    for (i = 0; i < 3; i++) {
        o << "[ ";
        for (j = 0; j < 3; j++) {
            o << A[i][j] << ", ";
        }
        o << "]" << endl;
    }
    return o;
}

/*******************************************************************************/
/* Geometry */
/*******************************************************************************/

/*
Vertex::Vertex(float _x, float _y, float _z) 
    : Vector(_x, _y, _z) {
}

ostream& operator<<(ostream &o, const Vertex &v) {
    o << "vertex(" << v.x << ", " << v.y << ", " << v.z << ")";
    return o;
}
*/

/******************************************************/

Face::Face(Vector *v0, Vector *v1, Vector *v2) {
    vertices[0] = v0;
    vertices[1] = v1;
    vertices[2] = v2;
    normals[0] = NULL;
    normals[1] = NULL;
    normals[2] = NULL;
    texcoords[0] = NULL;
    texcoords[1] = NULL;
    texcoords[2] = NULL;
}

Face::Face(const Face &f) {
    memcpy(vertices, f.vertices, sizeof(vertices));
    memcpy(normals, f.normals, sizeof(normals));
    memcpy(texcoords, f.texcoords, sizeof(texcoords));
}

Face& Face::operator=(const Face &f) {
    memcpy(vertices, f.vertices, sizeof(vertices));
    memcpy(normals, f.normals, sizeof(normals));
    memcpy(texcoords, f.texcoords, sizeof(texcoords));

    return *this;
}

ostream& operator<<(ostream &o, const Face &f) {
    o << "face vertices: " << *f.vertices[0] << ", " << *f.vertices[1] << ", " << *f.vertices[2] << endl;
    o << "face normals: " << *f.normals[0] << ", " << *f.normals[1] << ", " << *f.normals[2] << endl;
    
    return o;
}


/******************************************************/
/* ALGORITHMS */
/******************************************************/

Vector operator *(const float m[16], const Vector &v) {
    Vector r1(m[0], m[4], m[8]);
    Vector r2(m[1], m[5], m[9]);
    Vector r3(m[2], m[6], m[10]);
    
    return Vector( r1.dot(v), r2.dot(v), r3.dot(v) );
}

Vector intersect_ray_plane(Vector *P0, Vector *P1,
                           Vector *V0, Vector *n) {
    Vector segn(*P1 -*P0);
    float r1 = n->dot(*V0 - *P0) / n->dot(segn);
    return *P0 + segn * r1;
}

bool is_overlap(Vector &a, Vector &b1, Vector &b2) {
    if (b1.x != b2.x) {
        return ((b1.x < a.x) && (a.x < b2.x)) ||
            ((b1.x > a.x) && (a.x > b2.x));
    } else if (b1.y != b2.y) {
        return ((b1.y < a.y) && (a.y < b2.y)) ||
            ((b1.y > a.y) && (a.y > b2.y));
    } else if (b1.z != b2.z) {
        return ((b1.z < a.z) && (a.z < b2.z)) ||
            ((b1.z > a.z) && (a.z > b2.z));
    }

    return false;
}

bool intersect_triangle_plane(Vector *tv1, Vector *tv2, Vector *tv3, 
                              Vector *V0, Vector *Vn,
                              Vector &i1, Vector &i2) { 
    vector<Vector*> side1, side2;
    float d1, d2, d3;
    bool valid = true;
    d1 = dist_point_plane(tv1, V0, Vn);
    d2 = dist_point_plane(tv2, V0, Vn);
    d3 = dist_point_plane(tv3, V0, Vn);
    if (d1 > 0) side1.push_back(tv1); else if (d1 < 0) side2.push_back(tv1); else valid = false;
    if (d2 > 0) side1.push_back(tv2); else if (d2 < 0) side2.push_back(tv2); else valid = false;
    if (d3 > 0) side1.push_back(tv3); else if (d3 < 0) side2.push_back(tv3); else valid = false;

    if (!valid) return false;

    if (side1.size() == 1) {
        i1 = intersect_ray_plane(side1[0], side2[0], V0, Vn);
        i2 = intersect_ray_plane(side1[0], side2[1], V0, Vn);
        return true;
    }
    else if (side2.size() == 1) {
        i1 = intersect_ray_plane(side2[0], side1[0], V0, Vn);
        i2 = intersect_ray_plane(side2[0], side1[1], V0, Vn);
        return true;
    }

    return false;
}

bool intersect_triangle_triangle(Vector *tv11, Vector *tv12, Vector *tv13, Vector *tn1,
                                 Vector *tv21, Vector *tv22, Vector *tv23, Vector *tn2) {

    static int c;
    static Vector i11, i12, i21, i22;
    c = 0;

    // test if each triangle intersects the plane of the other
    if ( (intersect_triangle_plane(tv11, tv12, tv13, 
                                   tv21, tn2, 
                                   i11, i12)) &&
         (intersect_triangle_plane(tv21, tv22, tv23, 
                                   tv11, tn1, 
                                   i21, i22)) ) {

        // if the two triangle-plane intersection segments overlap, 
        // but *not* if one is entirely within the other,
        // then we have intersection
        if (is_overlap(i11, i21, i22)) c++;
        if (is_overlap(i12, i21, i22)) c++;
        if (c >= 2) return true;
        if (is_overlap(i21, i11, i12)) c++;          
        if (c >= 2) return true;
        if (is_overlap(i22, i11, i12)) c++;            
        if (c >= 2) return true;
    }

    // if at least one *does not* intersect the other's plane, there's no intersection
    return false;
} 

bool d_intersect_triangle_triangle(Vector *tv11, Vector *tv12, Vector *tv13, Vector *tn1,
                                   Vector *tv21, Vector *tv22, Vector *tv23, Vector *tn2,
                                   Vector i[2]) {

    static int c;
    static Vector i11, i12, i21, i22;
    c = 0;

    if ( (intersect_triangle_plane(tv11, tv12, tv13, 
                                   tv21, tn2, 
                                   i11, i12)) &&
         (intersect_triangle_plane(tv21, tv22, tv23, 
                                   tv11, tn1, 
                                   i21, i22)) ) {

        if (is_overlap(i11, i21, i22)) i[c++] = i11;
        if (is_overlap(i12, i21, i22)) i[c++] = i12;
        if (c >= 2) return true;
        if (is_overlap(i21, i11, i12)) i[c++] = i21;          
        if (c >= 2) return true;
        if (is_overlap(i22, i11, i12)) i[c++] = i22;
        if (c >= 2) return true;
    }

    return false;
} 


/* EOF */
